﻿using NLog;
using System.Text;
using TPXSDK.NET.GIS;
using TPXSDK.NET.Mission;
using TPXSDK.NET.Share;
using TPXSDK.NET.UObject;

namespace TPXSDK.NET.Link;

/// <summary>
/// 通讯链路协议类
/// </summary>
public class TPLink
{
    #region 成员变量
    /// <summary>
    /// TPLINK对象名称
    /// </summary>
    public string Name { get; set; }

    /// <summary>
    /// Link对象关联的设备ID(默认广播地址)
    /// </summary>
    public byte DevAddr { get; set; } = 0xFF;

    /// <summary>
    /// 当前关联通讯接口
    /// </summary>
    public CommAdapter? Comm { get; private set; }

    /// <summary>
    /// 响应报文数据
    /// </summary>
    public byte[]? RspData { get; private set; }

    /// <summary>
    /// 响应超时时间 (ms)
    /// </summary>
    public int RspTimeOut { get; set; } = 500;

    #region 私有成员
    /// <summary>
    /// 日志记录器
    /// </summary>
    private readonly Logger logger = LogManager.GetLogger("TPLink");

    /// <summary>
    /// 心跳包发送定时器
    /// </summary>
    private readonly System.Timers.Timer _heartBeatTmr;

    /// <summary>
    /// 数据接收缓冲区
    /// </summary>
    private readonly byte[] _rcvDataBuf = new byte[1024];

    /// <summary>
    /// 接收数据长度
    /// </summary>
    private int _rcvDataLen;

    /// <summary>
    /// 等待响应报文的序列号
    /// </summary>
    private volatile byte _waitPacketSn;

    /// <summary>
    /// 响应报文状态编码
    /// </summary>
    private volatile RspCode _rspCode = RspCode.RSP_UNKNOWN;

    /// <summary>
    /// 响应报文信号量
    /// </summary>
    private readonly SemaphoreSlim _rspSem = new(0);

    /// <summary>
    /// Link使用信号量，对于需要响应数据的操作，通过改信号量进行互斥处理
    /// </summary>
    private readonly SemaphoreSlim _linkSem = new(1);

    /// <summary>
    /// 请求命令超时重发次数
    /// </summary>
    private const int MaxRetry = 3;

    /// <summary>
    /// 接收到的报文时间戳（用于链路超时检测）
    /// </summary>
    private DateTime _rcvTimeStamp = DateTime.Now;
    #endregion

    #endregion

    public TPLink(string name)
    {
        _heartBeatTmr = new System.Timers.Timer(1000);
        _heartBeatTmr.Elapsed += delegate
        {
            SendCmdHeartBeat();
            // 检测是否有下行数据
            var dt = DateTime.Now - _rcvTimeStamp;
            if (dt.TotalMilliseconds > 5000)
            {
                PacketTimeOutEvent?.Invoke(this);
                logger.Error($"TPLINK Packet TimeOut， interval:{dt.TotalMilliseconds}");
            }
        };
        Name = name;
    }

    /// <summary>
    /// 打开并启动TPLink对象
    /// </summary>
    public bool Open(CommAdapter ca)
    {
        if (null == ca)
        {
            return false;
        }

        // 更新当前COMM对象
        if (null != Comm)
        {
            Comm.ReceivedEvent -= OnDataReceived;
            Comm.DelRefObj(this);
        }

        Comm = ca;
        Comm.ReceivedEvent += OnDataReceived;
        Comm.AddRefObj(this);

        if (!Comm.Open())
        {
            return false;
        }

        // 打开通信接口并启动心跳定时器
        HeartBeatEn = true;

        return true;
    }

    /// <summary>
    /// 关闭Link，关闭通讯适配器
    /// </summary>
    public void Close()
    {
        HeartBeatEn = false;
        if (IsOpened && null != Comm)
        {
            Comm.ReceivedEvent -= OnDataReceived;

            // 支持Comm对象共享使用场景，没有对Comm对象引用时，才能关闭
            if (0 == Comm.DelRefObj(this))
            {
                Comm.Close();
            }
        }
    }

    /// <summary>
    /// 心跳报文使能控制
    /// </summary>
    public bool HeartBeatEn
    {
        get => _heartBeatTmr.Enabled;
        set => _heartBeatTmr.Enabled = value;
    }

    /// <summary>
    /// 是否打开可进行正常收发
    /// </summary>
    public bool IsOpened => true == Comm?.IsOpened;

    /// <summary>
    /// 逐个字符输入分析，最后生成完整的报文信息
    /// </summary>
    private void PutChar(byte b)
    {
        if (0 == _rcvDataLen && LinkDef.Header1 != b)
        {
            return;// 过滤无效帧头数据
        }

        if (1 == _rcvDataLen && LinkDef.Header2 != b)
        {
            _rcvDataLen = 0;
            return;// 过滤无效帧头数据
        }

        _rcvDataBuf[_rcvDataLen++] = b;
        if (_rcvDataLen < LinkDef.MinPkSize)
        {
            return;
        }

        if (_rcvDataLen == _rcvDataBuf[2])
        {
            // 解析出完整的二进制报文
            if (Analyze(_rcvDataBuf, _rcvDataLen))
            {
                RcvDataTime = DateTime.Now;
            }

            // 清空接收缓冲区
            _rcvDataLen = 0;
        }

        // 防止接收缓冲区溢出
        if (_rcvDataLen > LinkDef.MaxPkSize - 2)
        {
            _rcvDataLen = 0;
        }
    }

    /// <summary>
    /// 对接收到的完整报文进行解析处理
    /// </summary>
    private bool Analyze(byte[] pData, int dataLen)
    {
        // 报文有效性验证
        if (false == CanAccept(pData, dataLen))
        {
            logger.Error($"Invalid Packet: {DataUtils.HexStr(pData, dataLen)}");
            return false;
        }

        // 更新接收时间戳
        _rcvTimeStamp = DateTime.Now;

        // 计算出负载长度（截掉帧头和校验字段长度）
        Header.SrcAddr = pData[3];
        Header.DstAddr = pData[4];
        Header.Sn = pData[5];
        Header.PayloadLen = (byte)(pData[2] - 8);

        // 过滤非本TPLink监听的地址
        if (0 != DevAddr && 0xFF != DevAddr && Header.SrcAddr != DevAddr)
        {
            return false;
        }

        // 回调接收到完整报文数据
        PacketRcvEvent?.Invoke(this, pData, dataLen);

        // 负载字段偏移字节数
        var payloadOft = 6;

        if (0x00 == Header.Sn)
        {
            ProcTelemetry(pData, Header.PayloadLen, payloadOft);
        }
        else if (0xFF == Header.Sn)
        {
            // 心跳帧 [建立设备地址表]
            if ((byte)LinkCmd.HEART_BEAT == pData[payloadOft])
            {
                payloadOft += 1;
                var uid = DataUtils.GEN_UINT32(pData, payloadOft);
                payloadOft += 4;
                var devType = (UDevType)pData[payloadOft];
                HeartBeatRcvEvent?.Invoke(this, devType, uid, Header.SrcAddr);
            }
            // 透传帧,废弃不再支持
        }
        else
        {
            // Response Packets
            ProcResponse(pData, Header.PayloadLen, payloadOft);
        }

        return true;
    }

    /// <summary>
    /// 处理响应报文
    /// </summary>
    private void ProcResponse(byte[] pData, int dataLen, int offset)
    {
        // 序号和类型判断
        if (_waitPacketSn != Header.Sn)
        {
            logger.Warn($"Invalid SN:{_waitPacketSn}!= {Header.Sn}");
            return;
        }

        // 解析响应状态码
        var rspLO = pData[offset++];
        var rspHI = pData[offset++];
        _rspCode = (RspCode)DataUtils.GEN_WORD(rspLO, rspHI);

        // 过滤响应码
        dataLen -= 2;

        // 复制负载数据
        RspData = new byte[dataLen];
        Array.Copy(pData, offset, RspData, 0, RspData.Length);

        // 通知RSP数据到达
        _rspSem.Release();
        logger.Info($"RCV RSP Code:{_rspCode}");
    }

    /// <summary>
    /// 处理遥测报文（标准遥测+自定义遥测+字符串遥测+状态码遥测...）
    /// </summary>
    internal void ProcTelemetry(byte[] pData, int dataLen, int offset)
    {
        var tlmType = (TlmType)pData[offset++];

        dataLen--;// 删除遥测类型
        if (tlmType <= TlmType.STD4)
        {
            ProcStdTelemetry(tlmType, pData, dataLen, offset);
        }
        else if (tlmType <= TlmType.USR4)
        {
            ProcUsrTelemetry(tlmType, pData, dataLen, offset);
        }
        else if (tlmType == TlmType.TXT)
        {
            var bText = new byte[dataLen];
            Array.Copy(pData, offset, bText, 0, dataLen);
            TlmTxt = Encoding.ASCII.GetString(bText).TrimEnd('\0');
        }
        else if (tlmType == TlmType.CODE)
        {
            var bText = new byte[dataLen];
            Array.Copy(pData, offset, bText, 0, dataLen);
            TlmCode = DataUtils.GEN_WORD(bText[0], bText[1]);
        }
        else if (tlmType == TlmType.BCST)
        {
            TlmBcst = DataUtils.BytesToStruct<BCST_TLM>(pData, offset, dataLen);
        }
        else if (tlmType == TlmType.GIMBAL)
        {
            TlmGimbal = DataUtils.BytesToStruct<GIMBAL_TLM>(pData, offset, dataLen);
        }
        else if (tlmType == TlmType.OBST)
        {
            // 多个障碍物对象解析（交给OBST）, FIXME
            if (dataLen > offset)
            {
                OBST = new byte[dataLen - offset];//FIXME
            }
        }
        else
        {
            return;
        }

        TlmRcvEvent?.Invoke(this, tlmType);
    }

    /// <summary>
    /// 更新标准遥测报文 0
    /// </summary>
    public void UpdateTlm(STD_TLM_0 tlm)
    {
        Tlm0 = tlm;
        TlmRcvEvent?.Invoke(this, TlmType.STD0);
    }

    /// <summary>
    /// 更新标准遥测报文 1
    /// </summary>
    public void UpdateTlm(STD_TLM_1 tlm)
    {
        Tlm1 = tlm;
        TlmRcvEvent?.Invoke(this, TlmType.STD1);
    }

    /// <summary>
    /// 更新标准遥测报文 2
    /// </summary>
    public void UpdateTlm(STD_TLM_2 tlm)
    {
        Tlm2 = tlm;
        TlmRcvEvent?.Invoke(this, TlmType.STD2);
    }

    /// <summary>
    /// 更新标准遥测报文 3
    /// </summary>
    public void UpdateTlm(STD_TLM_3 tlm)
    {
        Tlm3 = tlm;
        TlmRcvEvent?.Invoke(this, TlmType.STD3);
    }

    /// <summary>
    /// 更新标准遥测报文 4
    /// </summary>
    public void UpdateTlm(STD_TLM_4 tlm)
    {
        Tlm4 = tlm;
        TlmRcvEvent?.Invoke(this, TlmType.STD4);
    }

    /// <summary>
    /// 更新TEXT遥测报文
    /// </summary>
    public void UpdateTlm(string txt)
    {
        TlmTxt = txt;
        TlmRcvEvent?.Invoke(this, TlmType.TXT);
    }

    /// <summary>
    /// 处理标准遥测报文
    /// </summary>
    private void ProcStdTelemetry(TlmType tlmType, byte[] pData, int dataLen, int index)
    {
        switch (tlmType)
        {
            case TlmType.STD0: Tlm0 = DataUtils.BytesToStruct<STD_TLM_0>(pData, index, dataLen); break;
            case TlmType.STD1: Tlm1 = DataUtils.BytesToStruct<STD_TLM_1>(pData, index, dataLen); break;
            case TlmType.STD2: Tlm2 = DataUtils.BytesToStruct<STD_TLM_2>(pData, index, dataLen); break;
            case TlmType.STD3: Tlm3 = DataUtils.BytesToStruct<STD_TLM_3>(pData, index, dataLen); break;
            case TlmType.STD4: Tlm4 = DataUtils.BytesToStruct<STD_TLM_4>(pData, index, dataLen); break;
            default:
                throw new ArgumentOutOfRangeException(nameof(tlmType), tlmType, null);
        }
    }

    /// <summary>
    /// 处理自定义遥测报文
    /// </summary>
    private void ProcUsrTelemetry(TlmType tlmType, byte[] pData, int dataLen, int index)
    {
        var fieldCnt = pData[index++];

        // 解析FieldID集合
        var fieldIds = new uint[fieldCnt];
        for (var i = 0; i < fieldCnt; i++)
        {
            fieldIds[i] = DataUtils.GEN_UINT32(pData, index);
            index += 4;
        }

        // 获得FieldValue数据集合
        var valBytes = new byte[dataLen - 1 - 4 * fieldCnt];
        for (var i = 0; i < valBytes.Length; i++)
        {
            valBytes[i] = pData[index++];
        }

        // 通知委托函数处理
        UsrTlmRcvEvent?.Invoke(this, tlmType, fieldIds, valBytes);
    }

    /// <summary>
    /// 当通讯接口接收到数据后被调用,该函数应该尽快处理完毕退出
    /// 剩余完整报文处理事宜应该分配给协议分析线程处理
    /// </summary>
    private void OnDataReceived(CommAdapter sender, byte[] pData, int dataLen)
    {
        for (var i = 0; i < dataLen; i++)
        {
            PutChar(pData[i]);
        }
    }

    /// <summary>
    /// 报文有效性判断
    /// </summary>
    private bool CanAccept(byte[] pData, int dataLen)
    {
        if (LinkDef.Header1 != pData[0] ||
            LinkDef.Header2 != pData[1])
        {
            return false;
        }

        // 计算并验证校验和
        var crc = DataUtils.GEN_WORD(pData[dataLen - 2], pData[dataLen - 1]);
        var expCrc = CRC16.Calc(pData, 2, dataLen - 4);
        if (expCrc != crc)
        {
            logger.Error("CRC Error!");
        }
        return crc == expCrc;
    }

    /// <summary>
    /// 用于帧发送组装的辅助函数,包括：数据长度设定、增加帧头尾和校验和字节
    /// </summary>
    private byte[] GenSendData(byte[] payloadData, int payloadLen, byte sn, bool IsBroadcast = false)
    {
        var index = 0;
        var sndBuf = new byte[LinkDef.MaxPkSize];
        sndBuf[index++] = LinkDef.Header1;
        sndBuf[index++] = LinkDef.Header2;
        sndBuf[index++] = (byte)(payloadLen + 8); // 报文总长度
        sndBuf[index++] = 0x00;   // 报文源地址 【地面站地址，默认为0】
        sndBuf[index++] = IsBroadcast ? (byte)0xFF : DevAddr;// 报文目的地址
        sndBuf[index++] = sn;     // 报文序号

        for (var i = 0; i < payloadLen; i++)
        {
            sndBuf[index++] = payloadData[i];
        }

        var crc = CRC16.Calc(sndBuf, 2, index - 2);// 计算CRC校验码
        sndBuf[index++] = DataUtils.WORD_LO(crc);  // CRC低字节
        sndBuf[index++] = DataUtils.WORD_HI(crc);  // CRC高字节

        // Assert(index == payloadLen + 8);
        return sndBuf;
    }

    #region LINK_CMD函数集合
    /// <summary>
    /// 等待响应数据预处理函数
    /// </summary>
    private bool PrepareForRsp()
    {
        if (!_linkSem.Wait(RspTimeOut * 2))
        {
            return false;
        }
        RspData = [];
        _rspCode = RspCode.RSP_UNKNOWN;
        return true;
    }

    /// <summary>
    /// 向飞控发送RTCM数据，支持RTK功能
    /// </summary>
    /// <param name="data">RTCM格式差分修正数据</param>
    public void SendCmdRtcmUpdate(byte[] data)
    {
        if (!IsOpened) return;
        // 最大单包支持100字节
        if (data.Length > LinkDef.MaxPkSize - 28) return;

        var index = 0;

        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.RTCM_UPDATE;
        Array.Copy(data, 0, payload, index, data.Length);
        index += data.Length;
        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
    }

    /// <summary>
    /// 发送UOBJ读取指令
    /// </summary>
    public void SendCmdGetUObj(UObjBase uobj)
    {
        if (!IsOpened) return;
        var fieldId = UObjAdmin.FieldId(uobj);
        SendCmdGetField(fieldId);
        logger.Info($"SendCmdGetUObj:{uobj}");
    }

    /// <summary>
    /// 发送UOBJ读取指令【异步模式】
    /// </summary>
    public async Task<UObjBase?> DoCmdGetUObj(UObjBase? uobj)
    {
        if (null == uobj || !PrepareForRsp())
        {
            logger.Error("DoCmdGetUObj: Invalid Param");
            return null;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdGetUObj(uobj);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                // RspCode/RpsData不正确
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    logger.Error("DoCmdGetUObj: RSP not OK");
                    break;
                }

                // 负载数据长度不正确
                if (RspData.Length != uobj.Size + 4)
                {
                    logger.Error("DoCmdGetUObj: Invalid Rsp data length");
                    break;
                }

                // 判断FieldID是否匹配
                var fieldId = DataUtils.GEN_INT32(RspData);
                if (fieldId != UObjAdmin.FieldId(uobj))
                {
                    logger.Error("DoCmdGetUObj: Invalid Rsp fieldId");
                    break;
                }

                // 本地UObj对象更新
                if (!uobj.Update(RspData, 4))
                {
                    logger.Error("DoCmdGetUObj: Uobj data update fail");
                    break;
                }

                // 正确获得UOBj对象数据
                _linkSem.Release();
                return uobj;
            }
            // 超时重试
        }
        _linkSem.Release();
        return null;
    }

    /// <summary>
    /// 发送根据FieldName的读取指令
    /// </summary>
    public void SendCmdGetField(string fieldName)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.GET_FIELD_STR;
        var nm = Encoding.ASCII.GetBytes(fieldName);
        for (var i = 0; i < nm.Length; i++)
        {
            payload[index++] = nm[i];
        }
        payload[index++] = (byte)'\0';
        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdGetField: {fieldName}");
    }

    /// <summary>
    /// 发送Field读取指令【异步模式】
    /// </summary>
    public async Task<string> DoCmdGetField(string fieldName)
    {
        if (!PrepareForRsp())
        {
            return string.Empty;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdGetField(fieldName);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    break;
                }
                var rspStr = Encoding.ASCII.GetString(RspData);
                var segs = rspStr.Split('\0');
                if (segs.Length != 2)
                {
                    break;
                }

                var rcvFieldNm = segs[0];
                if (rcvFieldNm != fieldName)
                {
                    break;
                }

                // 分析获取结果,更新Admin数据内容
                _linkSem.Release();
                return segs[1];
            }
        }

        _linkSem.Release();
        return string.Empty;
    }

    /// <summary>
    /// 发送根据FieldID的读取指令
    /// </summary>
    public void SendCmdGetField(uint fieldId)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.GET_FIELD;
        payload[index++] = DataUtils.DWORD_B1(fieldId);
        payload[index++] = DataUtils.DWORD_B2(fieldId);
        payload[index++] = DataUtils.DWORD_B3(fieldId);
        payload[index++] = DataUtils.DWORD_B4(fieldId);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdGetField: 0x{fieldId:X8}");
    }

    /// <summary>
    /// 发送Field读取指令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdGetField(UObjAdmin admin, uint fieldId)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdGetField(fieldId);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    _linkSem.Release();
                    return _rspCode;
                }

                var rcvFieldId = DataUtils.GEN_UINT32(RspData);
                if (rcvFieldId != fieldId)
                {
                    _linkSem.Release();
                    return RspCode.RSP_FIELD_INVALID_ID;
                }

                // 分析获取结果,更新Admin数据内容
                var expectLen = admin.FieldSize(fieldId);
                if (RspData.Length != expectLen + 4)
                {
                    _linkSem.Release();
                    return RspCode.RSP_FIELD_INVALID_VAL;
                }

                if (admin.SetValue(fieldId, RspData, 4))
                {
                    _linkSem.Release();
                    return RspCode.RSP_OK;
                }
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送Field读取指令【异步模式】,【字符串方式】
    /// </summary>
    public async Task<RspCode> DoCmdGetField(UObjAdmin admin, string fieldPara)
    {
        var fieldId = admin.FieldId(fieldPara);
        if (fieldId == UObjBase.InvalidFieldId)
        {
            return RspCode.RSP_FIELD_INVALID_ID;
        }

        return await DoCmdGetField(admin, fieldId);
    }

    /// <summary>
    /// 发送UOBJ设置指令
    /// </summary>
    /// <param name="uobj">待设置对象</param>
    /// <param name="needRsp">是否需要响应报文</param>
    public void SendCmdSetUObj(UObjBase uobj, bool needRsp = true, bool save2Flash = false)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        var fieldId = UObjAdmin.FieldId(uobj);
        payload[index++] = save2Flash ? (byte)LinkCmd.SET_SAVE_FIELD : (byte)LinkCmd.SET_FIELD;
        payload[index++] = DataUtils.DWORD_B1(fieldId);
        payload[index++] = DataUtils.DWORD_B2(fieldId);
        payload[index++] = DataUtils.DWORD_B3(fieldId);
        payload[index++] = DataUtils.DWORD_B4(fieldId);

        var objData = uobj.ToBytes();
        if (null != objData)
        {
            foreach (var b in objData)
            {
                payload[index++] = b;
            }
        }

        var sndBuf = GenSendData(payload, index, AllocPacketSn(needRsp));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdSetUObj: {uobj} {objData?.Length}");
    }

    /// <summary>
    /// 发送UOBJ设置指令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSetUObj(UObjBase uobj, bool needRsp = true, bool save2Flash = false)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (var i = 0; i < MaxRetry; i++)
        {
            SendCmdSetUObj(uobj, needRsp, save2Flash);

            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送Field设置指令【字符串方式】
    /// </summary>
    public void SendCmdSetField(string fieldName, string fieldVal, bool save2Flash = false)
    {
        if (!IsOpened) return;
        fieldName = fieldName.Trim().TrimEnd('\0');
        fieldVal = fieldVal.Trim().TrimEnd('\0');
        if (!DataUtils.IsNumeric(fieldVal))
        {
            // log.Error($"Invalid FieldVal={fieldVal}");
            // return;
            // 支持非数值型字符串方式设置
        }

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = save2Flash ? (byte)LinkCmd.SET_SAVE_FIELD_STR : (byte)LinkCmd.SET_FIELD_STR;
        var nm = Encoding.ASCII.GetBytes(fieldName);
        for (var i = 0; i < nm.Length; i++)
        {
            payload[index++] = nm[i];
        }
        payload[index++] = (byte)'\0';

        var val = Encoding.ASCII.GetBytes(fieldVal);
        for (var i = 0; i < val.Length; i++)
        {
            payload[index++] = val[i];
        }
        payload[index++] = (byte)'\0';

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdSetField: {fieldName}={fieldVal}");

    }

    /// <summary>
    /// 发送Field设置指令【字符串方式,异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSetField(string fieldName, string fieldVal, bool save2Flash = false)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (var i = 0; i < MaxRetry; i++)
        {
            SendCmdSetField(fieldName, fieldVal, save2Flash);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送Field设置指令
    /// </summary>
    /// <param name="admin"></param>
    /// <param name="fieldId">待设置FieldID</param>
    /// <param name="val">待设置Field值</param>
    /// <param name="save2Flash">是否保存到FLASH中</param>
    public void SendCmdSetField(UObjAdmin admin, uint fieldId, object val, bool save2Flash = false)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = save2Flash ? (byte)LinkCmd.SET_SAVE_FIELD : (byte)LinkCmd.SET_FIELD;

        payload[index++] = DataUtils.DWORD_B1(fieldId);
        payload[index++] = DataUtils.DWORD_B2(fieldId);
        payload[index++] = DataUtils.DWORD_B3(fieldId);
        payload[index++] = DataUtils.DWORD_B4(fieldId);
        var valBytes = admin.FieldVal2Bytes(fieldId, val);
        if (null == valBytes)
        {
            throw new Exception($"Invalid Field Value: {fieldId}={val}");
        }
        Array.Copy(valBytes, (long)0, payload, index, valBytes.Length);
        index += valBytes.Length;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdSetField {fieldId}={val}");
    }

    /// <summary>
    /// 发送Field设置指令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSetField(UObjAdmin admin, uint fieldId, object val, bool save2Flash = false)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        SendCmdSetField(admin, fieldId, val, save2Flash);
        if (await _rspSem.WaitAsync(RspTimeOut))
        {
            _linkSem.Release();
            return _rspCode;
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送UObjMeta读取请求指令
    /// </summary>
    public void SendCmdGetUObjMeta(UObjMetaSubCmd subCmd, byte[]? para)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.READ_UOBJ_META;
        payload[index++] = (byte)subCmd;

        // 拷贝参数负载
        if (null != para)
        {
            foreach (var b in para)
            {
                payload[index++] = b;
            }
        }

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdGetUObjMeta");
    }

    /// <summary>
    /// 发送UObjMeta读取请求指令【异步模式】
    /// 请求Signature参数： NULL
    /// 请求ClassMeta参数： TypeID
    /// 请求MemberMeta参数：TypeID + MemID
    /// </summary>
    public async Task<RspCode> DoCmdGetUObjMeta(UObjMetaSubCmd subCmd, byte[]? para)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }
        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdGetUObjMeta(subCmd, para);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }
        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送指定域保存操作请求命令
    /// </summary>
    public void SendCmdSaveSetting(uint fieldId)
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.SAVE_SETTING;
        payload[index++] = DataUtils.DWORD_B1(fieldId);
        payload[index++] = DataUtils.DWORD_B2(fieldId);
        payload[index++] = DataUtils.DWORD_B3(fieldId);
        payload[index++] = DataUtils.DWORD_B4(fieldId);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdSaveSetting({fieldId})");
    }

    /// <summary>
    /// 发送指定域保存操作请求命令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSaveSetting(uint fieldId)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdSaveSetting(fieldId);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送指定域保存操作请求命令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSaveSetting()
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdSaveSetting();
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送所有参数保存操作请求命令
    /// </summary>
    public void SendCmdSaveSetting()
    {
        if (!IsOpened) return;

        var payload = new byte[LinkDef.MaxPkSize];
        payload[0] = (byte)LinkCmd.SAVE_SETTING;

        var sndBuf = GenSendData(payload, 1, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdSaveSetting(All)");
    }


    /// <summary>
    /// 发送参数保存到文件操作请求命令
    /// </summary>
    public void SendCmdSaveSettingFile()
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.SAVE_SETTING_FILE;
        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdSaveSettingFile");
    }


    /// <summary>
    /// 发送参数保存到文件操作请求命令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSaveSettingFile()
    {
        if (!_linkSem.Wait(RspTimeOut * 5))
        {
            return RspCode.RSP_TIMEOUT;
        }

        RspData = null;
        _rspCode = RspCode.RSP_UNKNOWN;

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdSaveSettingFile();
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }
    /// <summary>
    /// 发送目标位置点跟踪请求命令
    /// </summary>
    public void SendCmdFollowTarget(AbsPos pos, float[] vel)
    {
        if (!IsOpened) return;

        var payload = new byte[LinkDef.MaxPkSize];
        payload[0] = (byte)LinkCmd.FOLLOW_TARGET;
        var payloadLen = 1;
        var param = new int[3];
        param[0] = (int)(pos.Lat * 1e7);
        param[1] = (int)(pos.Lng * 1e7);
        param[2] = (int)(pos.Alt * 100);// 相对高度cm

        var b = DataUtils.GetBytes((uint)param[0]);
        Array.Copy(b, 0, payload, payloadLen, 4);
        payloadLen += 4;

        b = DataUtils.GetBytes((uint)param[1]);
        Array.Copy(b, 0, payload, payloadLen, 4);
        payloadLen += 4;

        b = DataUtils.GetBytes((uint)param[2]);
        Array.Copy(b, 0, payload, payloadLen, 4);
        payloadLen += 4;

        var velCm = (ushort)(vel[0] * 100);
        payload[payloadLen++] = DataUtils.WORD_HI(velCm);
        payload[payloadLen++] = DataUtils.WORD_LO(velCm);

        velCm = (ushort)(vel[1] * 100);
        payload[payloadLen++] = DataUtils.WORD_HI(velCm);
        payload[payloadLen++] = DataUtils.WORD_LO(velCm);

        velCm = (ushort)(vel[2] * 100);
        payload[payloadLen++] = DataUtils.WORD_HI(velCm);
        payload[payloadLen++] = DataUtils.WORD_LO(velCm);

        var sndBuf = GenSendData(payload, payloadLen, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdFollowTarget");
    }

    /// <summary>
    /// 发送任务查询指令（查询FlyAdmin）
    /// </summary>
    /// <param name="misId">待查询任务ID</param>
    public void SendCmdMissionQueryFlyAdmin(byte misId)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.MISSION_QUERY;
        payload[index++] = misId;
        payload[index++] = (byte)MisSubCmd.FLY_ADMIN;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdMissionQuery {misId}");
    }

    /// <summary>
    /// 发送任务查询指令（查询FlyAdmin）【异步模式】
    /// </summary>
    public async Task<ushort[]> DoCmdMissionQueryFlyAdmin(byte misId)
    {
        if (!PrepareForRsp())
        {
            return [0, 0];
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdMissionQueryFlyAdmin(misId);

            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    _linkSem.Release();
                    return [0, 0];
                }

                if (6 != RspData.Length
                    || RspData[0] != misId
                    || RspData[1] != (byte)MisSubCmd.FLY_ADMIN)
                {
                    _linkSem.Release();
                    return [0, 0];
                }

                _linkSem.Release();
                return [DataUtils.GEN_WORD(RspData[2], RspData[3]),
                        DataUtils.GEN_WORD(RspData[4], RspData[5])];
            }
        }
        _linkSem.Release();
        return [0, 0];
    }

    /// <summary>
    /// 发送任务查询指令（查询FlyItem）
    /// </summary>
    public void SendCmdMissionQueryFlyItem(byte misId, ushort step)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.MISSION_QUERY;
        payload[index++] = misId;
        payload[index++] = (byte)MisSubCmd.FLY_ITEM;
        payload[index++] = DataUtils.WORD_LO(step);
        payload[index++] = DataUtils.WORD_HI(step);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdMissionQuery {misId} {step}");
    }

    /// <summary>
    /// 发送任务查询指令【异步模式】
    /// </summary>
    public async Task<FlyCmdItem?> DoCmdMissionQueryFlyItem(byte misId, ushort step)
    {
        if (!PrepareForRsp())
        {
            return null;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdMissionQueryFlyItem(misId, step);

            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    _linkSem.Release();
                    return null;
                }

                // Check response data
                if (LinkDef.FlyCmdItemSize + 4 != RspData.Length
                    || RspData[0] != misId
                    || RspData[1] != 0x02//Subcode
                )
                {
                    _linkSem.Release();
                    return null;// RspCode.RSP_FIELD_INVALID_ID
                }

                var cmdStep = DataUtils.GEN_WORD(RspData[2], RspData[3]);
                if (cmdStep != step)
                {
                    _linkSem.Release();
                    return null;// RspCode.RSP_FIELD_INVALID_ID
                }

                var cmdItem = new FlyCmdItem();
                cmdItem.SetBytes(RspData, 4);

                _linkSem.Release();
                return cmdItem;
            }
        }

        _linkSem.Release();
        return null;
    }

    /// <summary>
    /// 发送任务编辑指令（编辑 FlyAdmin）
    /// </summary>
    public void SendCmdMissionEditFlyAdmin(byte misId, ushort count, ushort crc)
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.MISSION_EDIT;
        payload[index++] = misId;
        payload[index++] = (byte)MisSubCmd.FLY_ADMIN;
        payload[index++] = DataUtils.WORD_LO(count);
        payload[index++] = DataUtils.WORD_HI(count);

        payload[index++] = DataUtils.WORD_LO(crc);
        payload[index++] = DataUtils.WORD_HI(crc);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("MissionEdit: FlyAdmin");
    }

    /// <summary>
    /// 发送任务编辑指令（编辑 FlyAdmin）【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdMissionEditFlyAdmin(byte misId, ushort count, ushort crc)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdMissionEditFlyAdmin(misId, count, crc);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }
        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送任务编辑指令（编辑FlyItem）
    /// </summary>
    public void SendCmdMissionEditFlyItem(byte misId, ushort step, FlyCmdItem item)
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.MISSION_EDIT;
        payload[index++] = misId;
        payload[index++] = (byte)MisSubCmd.FLY_ITEM;
        payload[index++] = DataUtils.WORD_LO(step);
        payload[index++] = DataUtils.WORD_HI(step);

        var pData = item.GetBytes();
        Array.Copy(pData, (long)0, payload, index, pData.Length);
        index += pData.Length;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("Mission Edit: FlyCmd");
    }

    /// <summary>
    /// 发送任务编辑指令（编辑FlyCmdItem）【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdMissionEditFlyItem(byte misId, ushort step, FlyCmdItem item)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdMissionEditFlyItem(misId, step, item);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送任务编辑指令（任务加载切换）
    /// </summary>
    public void SendCmdMissionEditUpdate(byte misId)
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.MISSION_EDIT;
        payload[index++] = misId;
        payload[index++] = (byte)MisSubCmd.MIS_UPDATE;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("MissionEdit: MIS_UPDATE");
    }

    /// <summary>
    /// 发送任务编辑指令 任务加载切换【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdMissionEditUpdate(byte misId)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdMissionEditUpdate(misId);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }
        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送心跳报文
    /// </summary>
    public void SendCmdHeartBeat()
    {
        if (!IsOpened) return;
        if (!HeartBeatEn) return;

        var payload = new[] { (byte)LinkCmd.HEART_BEAT };
        var sndBuf = GenSendData(payload, payload.Length, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        //log.Info("SendCmdHeartBeat");
    }

    /// <summary>
    /// 发送飞行模式设置指令
    /// </summary>
    /// <param name="fm">目标FlightMode</param>
    /// <param name="fs">目标FlightStage</param>
    public void SendCmdSetFlightMode(byte fm, byte fs)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];

        payload[index++] = (byte)LinkCmd.SET_FLIGHT_MODE;
        payload[index++] = (byte)(fs << 4 | fm);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdSetFlightMode");
    }

    /// <summary>
    /// 发送飞行模式设置指令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdSetFlightMode(byte fm, byte fs)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdSetFlightMode(fm, fs);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 发送进入U盘模式指令
    /// </summary>
    public void SendCmdEnterUDisk()
    {
        if (!IsOpened) return;
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.ENTER_UDISK;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(true));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdEnterUDisk");
    }

    #region 固件升级相关指令
    /// <summary>
    /// 发送进入IAP模式指令（固件升级）
    /// </summary>
    public void SendCmdEnterIAP()
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.ENTER_IAP;
        payload[index++] = (byte)'X';
        payload[index++] = (byte)'I';
        payload[index++] = (byte)'A';
        payload[index++] = (byte)'P';

        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdEnterIAP");
    }

    /// <summary>
    /// 发送Flash擦除请求指令
    /// </summary>
    /// <param name="imgSize">镜像大小</param>
    /// <param name="imgCRC">镜像校验码</param>
    public void SendCmdEraseFlash(uint imgSize, uint imgCRC)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.ERASE_FLASH;

        payload[index++] = (byte)(imgSize & 0xFF);
        payload[index++] = (byte)(imgSize >> 8 & 0xFF);
        payload[index++] = (byte)(imgSize >> 16 & 0xFF);
        payload[index++] = (byte)(imgSize >> 24 & 0xFF);

        payload[index++] = (byte)(imgCRC & 0xFF);
        payload[index++] = (byte)(imgCRC >> 8 & 0xFF);
        payload[index++] = (byte)(imgCRC >> 16 & 0xFF);
        payload[index++] = (byte)(imgCRC >> 24 & 0xFF);

        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info("SendCmdEraseFlash");
    }

    /// <summary>
    /// 发送镜像数据下载指令
    /// </summary>
    /// <param name="data">镜像数据</param>
    /// <param name="len">数据长度</param>
    /// <param name="packSeq">包编码</param>
    public void SendCmdDownBin(byte[] data, int len, uint packSeq)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.DOWNLOAD_BIN;

        // packet Seq 4Byte, packSeq == 0xFFFFFFFF 代表data数据内容是MD5验证码
        payload[index++] = (byte)(packSeq & 0xFF);
        payload[index++] = (byte)(packSeq >> 8 & 0xFF);
        payload[index++] = (byte)(packSeq >> 16 & 0xFF);
        payload[index++] = (byte)(packSeq >> 24 & 0xFF);

        Array.Copy(data, 0, payload, index, len);
        index += len;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);

        logger.Info($"SendCmdDownBin:{packSeq} {len}");
    }
    #endregion

    /// <summary>
    /// 发送Joystick按钮状态信息
    /// </summary>
    public void SendCmdJoystickUpdate(byte btns)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.JOYSTICK_UPDATE;
        payload[index++] = (byte)JoySubCmd.BTNS;
        payload[index++] = btns;

        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdJoysticUpdate:{btns}");
    }

    /// <summary>
    /// 发送Joystick轴状态信息
    /// </summary>
    public void SendCmdJoystickUpdate(sbyte[] axies)
    {
        if (!IsOpened || axies.Length != 5) return;
        // 最多支持5个轴【待完善】
        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.JOYSTICK_UPDATE;
        payload[index++] = (byte)JoySubCmd.AXIS;

        for (var i = 0; i < axies.Length; i++)
        {
            payload[index++] = (byte)axies[i];
        }

        var sndBuf = GenSendData(payload, index, AllocPacketSn(false));
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdJoysticUpdate:{axies}");
    }

    /// <summary>
    /// 发送DoAction指令
    /// </summary>
    /// <param name="subCmd">子级命令编码</param>
    /// <param name="para">命令参数</param>
    /// <param name="needRsp">是否需要反馈信息</param>
    public void SendCmdDoAction(ActionSubCmd subCmd, byte[]? para = null, bool needRsp = false, bool IsBroadCast = false)
    {
        if (!IsOpened) return;

        var index = 0;
        var payload = new byte[LinkDef.MaxPkSize];
        payload[index++] = (byte)LinkCmd.DO_ACTION;
        payload[index++] = (byte)subCmd;// 子级命令编码
        if (null != para && para.Length > 0)
        {
            Array.Copy(para, 0, payload, index, para.Length);
            index += para.Length;
        }

        var sndBuf = GenSendData(payload, index, AllocPacketSn(needRsp), IsBroadCast);
        Comm?.Write(sndBuf, 0, sndBuf[2]);
        logger.Info($"SendCmdDoAction:{subCmd}");
    }

    /// <summary>
    /// 发送DoAction指令【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdDoAction(ActionSubCmd subCmd, byte[] para, bool needRsp = false, bool isbroadcast = false)
    {
        if (needRsp)
        {
            if (!PrepareForRsp())
            {
                return RspCode.RSP_TIMEOUT;
            }

            for (int i = 0; i < MaxRetry; i++)
            {
                SendCmdDoAction(subCmd, para, needRsp, isbroadcast);
                if (await _rspSem.WaitAsync(RspTimeOut))
                {
                    _linkSem.Release();
                    return _rspCode;
                }
            }
            _linkSem.Release();
            return RspCode.RSP_TIMEOUT;
        }

        // 单次发送，不需要响应报文
        SendCmdDoAction(subCmd, para, needRsp, isbroadcast);
        return RspCode.RSP_OK;
    }

    #region DoAction指令工具函数
    /// <summary>
    /// DoAction指令工具函数-指点返航
    /// </summary>
    public void SendCmdRTL(double lat, double lng, float alt)
    {
        var param = new int[3];
        param[0] = (int)(lat * 1e7);
        param[1] = (int)(lng * 1e7);
        param[2] = (int)(alt * 100);

        var paramBytes = new byte[12];

        var b = DataUtils.GetBytes((uint)param[0]);
        Array.Copy(b, 0, paramBytes, 0, 4);

        b = DataUtils.GetBytes((uint)param[1]);
        Array.Copy(b, 0, paramBytes, 4, 4);

        b = DataUtils.GetBytes((uint)param[2]);
        Array.Copy(b, 0, paramBytes, 8, 4);

        SendCmdDoAction(ActionSubCmd.RTL, paramBytes);
    }

    /// <summary>
    /// 照明系统 控制请求指令
    /// </summary>
    public void SendCmdLightCtrl(bool isOn)
    {
        SendCmdDoAction(ActionSubCmd.LIGHT, isOn ? [1] : [0]);
    }

    /// <summary>
    /// 降落伞开伞控制请求指令
    /// </summary>
    public void SendCmdChute()
    {
        SendCmdDoAction(ActionSubCmd.CHUTE);
    }

    /// <summary>
    /// GPIO 控制请求指令
    /// </summary>
    public void SendCmdIOCtrl(int pin, bool isSet)
    {
        SendCmdDoAction(ActionSubCmd.IOCTRL, isSet ? [(byte)pin, 1] : [(byte)pin, 0]);
    }

    /// <summary>
    /// PWM 输出控制请求指令
    /// </summary>
    public void SendCmdPWMCtrl(int ch, ushort plus)
    {
        SendCmdDoAction(ActionSubCmd.PWMCTRL, [(byte)ch, DataUtils.WORD_LO(plus), DataUtils.WORD_HI(plus)]);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-拍照
    /// </summary>
    public void SendCmdPodSnap()
    {
        var param = new byte[10];
        param[0] = (byte)PodSubCmd.SNAP;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-设置变倍率
    /// </summary>
    public void SendCmdPodSetZoom(int zoom)
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.ZOOM_SET;
        param[1] = (byte)zoom;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-持续放大
    /// </summary>
    public void SendCmdPodZoomIn()
    {
        var param = new byte[1];
        param[0] = (byte)PodSubCmd.ZOOM_IN;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-持续缩小
    /// </summary>
    public void SendCmdPodZoomOut()
    {
        var param = new byte[1];
        param[0] = (byte)PodSubCmd.ZOOM_OUT;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-停止变倍缩放
    /// </summary>
    public void SendCmdPodZoomStop()
    {
        var param = new byte[1];
        param[0] = (byte)PodSubCmd.ZOOM_STOP;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-设置目标角度
    /// </summary>
    public void SendCmdPodSetAngle(float yaw, float pitch)
    {
        var param = new byte[10];
        param[0] = (byte)PodSubCmd.ANGLE;
        param[1] = 0;

        var b = BitConverter.GetBytes(yaw);
        Array.Copy(b, 0, param, 2, 4);

        b = BitConverter.GetBytes(pitch);
        Array.Copy(b, 0, param, 6, 4);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-录像
    /// </summary>
    public void SendCmdPodRecordCtrl(bool enable)
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.RECORD;
        param[1] = (byte)(enable ? 1 : 0);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-测距
    /// </summary>
    public void SendCmdPodLidarCtrl(bool enable)
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.LIDAR;
        param[1] = (byte)(enable ? 1 : 0);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱控制-OSD
    /// </summary>
    public void SendCmdPodOSDCtrl(bool enable)
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.OSD;
        param[1] = (byte)(enable ? 1 : 0);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-上
    /// </summary>
    public void SendCmdPodMoveUp()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_UP;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-下
    /// </summary>
    public void SendCmdPodMoveDown()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_DOWN;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-左
    /// </summary>
    public void SendCmdPodMoveLeft()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_LEFT;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-右
    /// </summary>
    public void SendCmdPodMoveRight()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_RIGHT;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-停止
    /// </summary>
    public void SendCmdPodMoveStop()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_STOP;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱移动-归中
    /// </summary>
    public void SendCmdPodMoveZero()
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.MOVE;
        param[1] = (byte)PodMoveCode.MOVE_ZERO;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-吊舱视频源设置
    /// </summary>
    public void SendCmdPodSetVideo(int videoSrc)
    {
        var param = new byte[2];
        param[0] = (byte)PodSubCmd.VIDEOSRC;
        param[1] = (byte)videoSrc;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-跟踪控制
    /// </summary>
    public void SendCmdPodTraceCtrl(bool enable, float x = 0, float y = 0)
    {
        var param = new byte[10];
        param[0] = (byte)PodSubCmd.TRACE;
        param[1] = (byte)(enable ? 1 : 0);

        var b = BitConverter.GetBytes(x);
        Array.Copy(b, 0, param, 2, 4);

        b = BitConverter.GetBytes(y);
        Array.Copy(b, 0, param, 6, 4);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }
    /// <summary>
    /// DoAction指令工具函数-跟踪控制 [根据跟踪对象ID]
    /// </summary>
    public void SendCmdPodTraceCtrl(uint objID)
    {
        var param = new byte[6];
        param[0] = (byte)PodSubCmd.TRACE_OBJ;
        param[1] = 0x00;// 备用

        var b = BitConverter.GetBytes(objID);
        Array.Copy(b, 0, param, 2, 4);

        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-设置吊舱IP地址
    /// </summary>
    public void SendCmdPodSetIP(bool dhcp, string strIP, string strSubnetMask, string strGateway)
    {
        var param = new byte[6 + 4 + 4];
        param[0] = (byte)PodSubCmd.SETADDR;
        param[1] = (byte)(dhcp ? 1 : 0);

        if (System.Net.IPAddress.TryParse(strIP, out System.Net.IPAddress? ipAddr))
        {
            var bts = ipAddr.GetAddressBytes();
            Array.Copy(bts, 0, param, 2, 4);
        }

        Array.Clear(param, 6, 4);
        if (System.Net.IPAddress.TryParse(strSubnetMask, out System.Net.IPAddress? subnetMask))
        {
            var bts = subnetMask.GetAddressBytes();
            Array.Copy(bts, 0, param, 6, 4);
        }
        Array.Clear(param, 10, 4);
        if (System.Net.IPAddress.TryParse(strGateway, out System.Net.IPAddress? gateway))
        {
            var bts = gateway.GetAddressBytes();
            Array.Copy(bts, 0, param, 10, 4);
        }
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-读取吊舱IP地址
    /// </summary>
    public void SendCmdPodGetIP()
    {
        var param = new byte[1];
        param[0] = (byte)PodSubCmd.GETADDR;
        SendCmdDoAction(ActionSubCmd.POD_CTRL, param);
    }

    /// <summary>
    /// DoAction指令工具函数-跟飞启停控制
    /// </summary>
    public void SendCmdTraceObj(bool enabled)
    {
        var param = new byte[1];
        param[0] = enabled ? (byte)1 : (byte)0;
        SendCmdDoAction(ActionSubCmd.TRACE_OBJ, param);
    }

    /// <summary>
    /// DoAction指令工具函数-读取TagMessage
    /// </summary>
    public void SendCmdReadTagMsg(int tagId)
    {
        var para = new byte[2];
        para[0] = 0;//Read Cmd
        para[1] = (byte)tagId;
        SendCmdDoAction(ActionSubCmd.TAG_MSG, para, true);
    }

    /// <summary>
    /// DoAction指令工具函数-写入TagMessage
    /// </summary>
    public void SendCmdWriteTagMsg(int tagId, string strMsg)
    {
        var byteArray = Encoding.ASCII.GetBytes(strMsg);
        var para = new byte[2 + byteArray.Length];
        para[0] = 1;//Write Cmd
        para[1] = (byte)tagId;
        for (int i = 0; i < byteArray.Length; i++)
        {
            para[2 + i] = byteArray[i];
        }
        SendCmdDoAction(ActionSubCmd.TAG_MSG, para, true);
    }

    /// <summary>
    /// DoAction指令工具函数-读取TagMessage【异步模式】
    /// </summary>
    public async Task<string> DoCmdReadTagMsg(int tagId)
    {
        if (!PrepareForRsp())
        {
            return string.Empty;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdReadTagMsg(tagId);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                if ((ushort)RspCode.RSP_OK != _rspCode || null == RspData)
                {
                    _linkSem.Release();
                    return string.Empty;
                }

                // 负载数据包含<32字节的字符串
                if (RspData.Length < 2 || RspData[0] != tagId)
                {
                    _linkSem.Release();
                    return string.Empty;
                }

                // 拷贝字符数组并转为字符串
                var byteArray = new byte[RspData.Length];
                Array.Copy(RspData, 1, byteArray, 0, RspData.Length - 1);
                byteArray[^1] = (byte)'\0';

                _linkSem.Release();
                return Encoding.Default.GetString(byteArray);
            }
        }

        _linkSem.Release();
        return string.Empty;
    }

    /// <summary>
    /// DoAction指令工具函数-写入TagMessage【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdWriteTagMsg(int tagId, string tagMsg)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdWriteTagMsg(tagId, tagMsg);
            if (await _rspSem.WaitAsync(RspTimeOut)) return _rspCode;
        }

        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-进入引导模式
    /// </summary>
    public void SendCmdEnterGuide(double lat, double lng, double relAlt, int radius = 0)
    {
        var param = new int[4];
        param[0] = (int)(lat * 1e7);
        param[1] = (int)(lng * 1e7);
        param[2] = (int)(relAlt * 100);//相对高度cm
        param[3] = radius * 100;// 引导点半径cm 【正负号代表盘旋方向】

        var paramBytes = new byte[16];

        var b = DataUtils.GetBytes((uint)param[0]);
        Array.Copy(b, 0, paramBytes, 0, 4);

        b = DataUtils.GetBytes((uint)param[1]);
        Array.Copy(b, 0, paramBytes, 4, 4);

        b = DataUtils.GetBytes((uint)param[2]);
        Array.Copy(b, 0, paramBytes, 8, 4);

        b = DataUtils.GetBytes((uint)param[3]);
        Array.Copy(b, 0, paramBytes, 12, 4);

        SendCmdDoAction(ActionSubCmd.GUIDE, paramBytes, true);
    }

    /// <summary>
    /// DoAction指令工具函数-进入引导模式【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdEnterGuide(double lat, double lng, double relAlt, int radius = 0)
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdEnterGuide(lat, lng, relAlt, radius);
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-离开引导模式
    /// </summary>
    public void SendCmdExitGuide()
    {
        SendCmdDoAction(ActionSubCmd.GUIDE, [], true);
    }

    /// <summary>
    /// DoAction指令工具函数-进入Hover模式
    /// </summary>
    public void SendCmdEnterHover()
    {
        SendCmdDoAction(ActionSubCmd.HOVER, [1], true);
    }

    /// <summary>
    /// DoAction指令工具函数-进入Hover模式【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdEnterHover()
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdEnterHover();
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-离开Hover模式
    /// </summary>
    public void SendCmdExitHover()
    {
        SendCmdDoAction(ActionSubCmd.HOVER, [0], true);
    }


    /// <summary>
    /// DoAction指令工具函数-更新Hover目标位置
    /// </summary>
    public void SendCmdUpdateHoverLoc(double lat, double lng, double relAlt)
    {
        var param = new int[3];
        param[0] = (int)(lat * 1e7);
        param[1] = (int)(lng * 1e7);
        param[2] = (int)(relAlt * 100);//相对高度cm

        var paramBytes = new byte[12];

        var b = DataUtils.GetBytes((uint)param[0]);
        Array.Copy(b, 0, paramBytes, 0, 4);

        b = DataUtils.GetBytes((uint)param[1]);
        Array.Copy(b, 0, paramBytes, 4, 4);

        b = DataUtils.GetBytes((uint)param[2]);
        Array.Copy(b, 0, paramBytes, 8, 4);

        SendCmdDoAction(ActionSubCmd.HOVER, paramBytes, true);
    }

    /// <summary>
    /// DoAction指令工具函数-更新Hover目标位置,不改变高度
    /// </summary>
    public void SendCmdUpdateHoverLoc(double lat, double lng)
    {
        var param = new int[2];
        param[0] = (int)(lat * 1e7);
        param[1] = (int)(lng * 1e7);

        var paramBytes = new byte[8];

        var b = DataUtils.GetBytes((uint)param[0]);
        Array.Copy(b, 0, paramBytes, 0, 4);

        b = DataUtils.GetBytes((uint)param[1]);
        Array.Copy(b, 0, paramBytes, 4, 4);

        SendCmdDoAction(ActionSubCmd.HOVER, paramBytes, true);
    }

    /// <summary>
    /// DoAction指令工具函数-更新Hover目标高度
    /// </summary>
    /// <param name="relAlt">新的相对目标高度</param>
    public void SendCmdUpdateHoverAlt(double relAlt)
    {
        var param = (int)(relAlt * 100);//相对高度cm
        var paramBytes = new byte[4];

        var b = DataUtils.GetBytes((uint)param);
        Array.Copy(b, 0, paramBytes, 0, 4);

        SendCmdDoAction(ActionSubCmd.HOVER, paramBytes, true);
    }

    /// <summary>
    /// DoAction指令工具函数-更新Hover模式目标位置【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdUpdateHoverLoc(double lat, double lng, double relAlt = double.NaN)
    {
        if (!_linkSem.Wait(RspTimeOut * 2))
        {
            return RspCode.RSP_TIMEOUT;
        }

        RspData = null;
        _rspCode = RspCode.RSP_UNKNOWN;

        if (double.IsNaN(relAlt))
        {
            SendCmdUpdateHoverLoc(lat, lng);
        }
        else
        {
            SendCmdUpdateHoverLoc(lat, lng, relAlt);
        }

        if (await _rspSem.WaitAsync(RspTimeOut))
        {
            _linkSem.Release();
            return _rspCode;
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-更新Hover目标高度（异步模式）
    /// </summary>
    public async Task<RspCode> DoCmdUpdateHoverAlt(double relAlt)
    {
        if (!_linkSem.Wait(RspTimeOut * 2))
        {
            return RspCode.RSP_TIMEOUT;
        }

        RspData = null;
        _rspCode = RspCode.RSP_UNKNOWN;

        SendCmdUpdateHoverAlt(relAlt);

        if (await _rspSem.WaitAsync(RspTimeOut))
        {
            _linkSem.Release();
            return _rspCode;
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-进入Hover模式【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdExitHover()
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdExitHover();
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// DoAction指令工具函数-退出引导模式【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdExitGuide()
    {
        if (!PrepareForRsp())
        {
            return RspCode.RSP_TIMEOUT;
        }

        for (int i = 0; i < MaxRetry; i++)
        {
            SendCmdExitGuide();
            if (await _rspSem.WaitAsync(RspTimeOut))
            {
                _linkSem.Release();
                return _rspCode;
            }
        }

        _linkSem.Release();
        return RspCode.RSP_TIMEOUT;
    }

    /// <summary>
    /// 启动磁罗盘校准功能【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdStartMagCalib()
    {
        return await DoCmdDoAction(ActionSubCmd.CALIB, [(byte)CalibParam.CALIB_PARAM_MAG, 1]);
    }

    /// <summary>
    /// 停止磁罗盘校准功能【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdStopMagCalib()
    {
        return await DoCmdDoAction(ActionSubCmd.CALIB, [(byte)CalibParam.CALIB_PARAM_MAG, 0]);
    }

    /// <summary>
    /// 停止电调校准功能【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdStopEscCalib()
    {
        return await DoCmdDoAction(ActionSubCmd.CALIB, [(byte)CalibParam.CALIB_PARAM_ESC, 0]);
    }

    /// <summary>
    /// 设置电调校准指令-输出最大信号【异步模式】
    /// </summary>
    public async Task<RspCode> DoCmdMaxOutEscCalib()
    {
        return await DoCmdDoAction(ActionSubCmd.CALIB, [(byte)CalibParam.CALIB_PARAM_ESC, 0xE2]);
    }

    /// <summary>
    /// 设置电调校准指令-输出最小信号【异步模式】
    /// </summary>
    /// <returns></returns>
    public async Task<RspCode> DoCmdMinOutEscCalib()
    {
        return await DoCmdDoAction(ActionSubCmd.CALIB, [(byte)CalibParam.CALIB_PARAM_ESC, 0xE1]);
    }

    /// <summary>
    /// 发送测试拍照相机指令 - 【无响应报文】
    /// </summary>
    public void SendCmdTestCamera()
    {
        var param = new byte[1];
        param[0] = 0x00;// 测试对象-相机

        SendCmdDoAction(ActionSubCmd.TEST, param);
    }

    /// <summary>
    /// 发送执行器测试指令 - 【无响应报文】
    /// </summary>
    /// <param name="actCode">测试项</param>
    public void SendCmdTestActuator(byte actCode)
    {
        var param = new byte[2];
        param[0] = 0x00;// 测试对象-执行器
        param[1] = actCode;

        SendCmdDoAction(ActionSubCmd.TEST, param);
    }

    #endregion//DoAction指令工具函数
    #endregion//LINK_CMD集合

    /// <summary>
    /// 分配报文序列号（不需要响应帧的SN固定为0xFF）
    /// </summary>
    private byte AllocPacketSn(bool needRsp)
    {
        if (!needRsp) return 0xFF;

        _allocPacketSn++;

        // 序列号0x00 预留给遥测报文
        if (0 == _allocPacketSn) _allocPacketSn = 1;

        // 序列号0xFF 预留给心跳报文和其他不需要响应的报文
        if (0xFF == _allocPacketSn) _allocPacketSn = 1;

        // 如果该报文等待响应，记录待响应报文序列号
        _waitPacketSn = _allocPacketSn;

        // 清除多余的信号量计数
        while (_rspSem.CurrentCount > 0)
        {
            _rspSem.Wait();
        }

        return _allocPacketSn;
    }

    #region 成员变量
    /// <summary>
    /// 报文序号分配记录值
    /// </summary>
    private byte _allocPacketSn;

    /// <summary>
    /// 最后收到有效数据报文时间戳
    /// </summary>
    public DateTime RcvDataTime { get; set; }

    /// <summary>
    ///  标准遥测报文0缓存
    /// </summary>
    public STD_TLM_0 Tlm0 { get; private set; }

    /// <summary>
    ///  标准遥测报文1缓存
    /// </summary>
    public STD_TLM_1 Tlm1 { get; private set; }

    /// <summary>
    ///  标准遥测报文2缓存
    /// </summary>
    public STD_TLM_2 Tlm2 { get; private set; }

    /// <summary>
    ///  标准遥测报文3缓存
    /// </summary>
    public STD_TLM_3 Tlm3 { get; private set; }

    /// <summary>
    ///  标准遥测报文4缓存
    /// </summary>
    public STD_TLM_4 Tlm4 { get; private set; }

    /// <summary>
    /// 吊舱遥测报文缓存
    /// </summary>
    public GIMBAL_TLM TlmGimbal { get; private set; }

    /// <summary>
    ///  TEXT遥测报文缓存
    /// </summary>
    public string? TlmTxt { get; private set; }

    /// <summary>
    ///  广播遥测报文缓存(集群编队)
    /// </summary>
    public BCST_TLM TlmBcst { get; private set; }

    /// <summary>
    ///  遥测报文状态字缓存
    /// </summary>
    public ushort TlmCode { get; private set; }

    /// <summary>
    /// 障碍物对象数据（个数+数值）
    /// </summary>
    public byte[]? OBST { get; private set; }

    /// <summary>
    /// 当前最新帧头数据内容（不含前导码）
    /// </summary>
    public FrameHeader Header { get; private set; } = new FrameHeader();

    #endregion

    #region 事件
    public delegate void PacketRcvHandler(TPLink lnk, byte[] data, int len);
    public delegate void PacketTimeOutHandler(TPLink lnk);
    public delegate void TlmRcvHandler(TPLink lnk, TlmType tlmType);
    public delegate void UsrTlmRcvHandler(TPLink lnk, TlmType tlmType, uint[] field, byte[] valBytes);
    public delegate void HeartBeatRcvHandler(TPLink lnk, UDevType t, uint uid, byte devid);
    public delegate void PassThroughHandler(TPLink lnk, byte src, byte dst, byte[] data);

    /// <summary>
    /// 接收到完整报文事件
    /// </summary>
    public event PacketRcvHandler? PacketRcvEvent;

    /// <summary>
    /// 接收报文超时事件（链路断开判断）
    /// </summary>
    public event PacketTimeOutHandler? PacketTimeOutEvent;

    /// <summary>
    /// 接收到标准遥测报文事件
    /// </summary>
    public event TlmRcvHandler? TlmRcvEvent;

    /// <summary>
    /// 接收到用户自定义遥测报文
    /// </summary>
    public event UsrTlmRcvHandler? UsrTlmRcvEvent;

    /// <summary>
    /// 心跳报文事件
    /// </summary>
    public event HeartBeatRcvHandler? HeartBeatRcvEvent;

    #endregion
}
